#include "can.h"
#include "cmsis_os.h"
#include "usart.h"
#include "Remote.h"
#include "Comm_CAN.h"
#include "pid.h"
#include "main.h"
#include "Solution.h"

extern motor_t motor_6020;
extern motor_t M3508_1;
extern motor_t M3508_2;
extern motor_t M3508_3;
extern motor_t M3508_4;
extern motor_t J4310;

extern RAW_Speed Chassis_Speed;

extern CAN_TxHeaderTypeDef motor_tx_message;
extern uint8_t motor_can_send_data[8];

extern RC_t RC;

extern pid_param speed;
extern pid_param angle;
extern pid_param M3508_speed_1;
extern pid_param M3508_speed_2;
extern pid_param M3508_speed_3;
extern pid_param M3508_speed_4;

void motor_task(void const * argument)
{
  can_filter_init();

  
  while (1) {
    // PID_control(&angle,motor_6020.angle_ecd,0);
    // PID_angle_control(&speed,motor_6020.speed_rpm,angle.out);

    Calc_Speed(&Chassis_Speed,RC);

    PID_control(&M3508_speed_1,M3508_1.speed_rpm,Chassis_Speed.V1);
    PID_control(&M3508_speed_2,M3508_2.speed_rpm,Chassis_Speed.V2);
    PID_control(&M3508_speed_3,M3508_3.speed_rpm,Chassis_Speed.V3);
    PID_control(&M3508_speed_4,M3508_4.speed_rpm,Chassis_Speed.V4);

    cmd_motor(0X1FF, 0, 0, 0, 0);
    if(RC.s1==2 && RC.s2 ==2){
      cmd_motor(0X200,M3508_speed_1.out,M3508_speed_2.out,M3508_speed_3.out,M3508_speed_4.out);
    }

    // DM_Speed_cmd(0x201,1.1);

    osDelay(1);
  }
}

